#ifndef COVER_DETECTOR_HPP
#define COVER_DETECTOR_HPP

#include "detector_base.hpp"
#include <unordered_map>
#include <geometry_msgs/msg/point_stamped.hpp>


namespace center_detection {

    struct CoverDetectionParams
    {
        //把手中心计算参数
        float handle_search_start_z = 0.6f;
        float handle_search_end_z = 0.3f;
        float handle_search_step = 0.01f;
        float handle_height_range = 0.15f;
        int handle_min_points = 200;
        float handle_z_offset = 0.15f;

        //井盖中心计算参数
        float circle_search_start_z = 0.33f;
        float circle_search_end_z = 0.2f;
        float circle_search_step = 0.01f;
        float circle_height_range = 0.1f;
        int circle_min_points = 50;
        int circle_min_points_final = 30;

        //中心计算参数
        float quantization_step = 0.01f;
        float percentile_low = 0.05f;
        float percentile_high = 0.95f;
        float center_merge_threshold = 0.02f;
    
    };

    class CoverDetector : public DetectorBase{

        public:

        CoverDetector() = default;
        ~CoverDetector() = default;

        bool detectCenter(PointCloud cloud, geometry_msgs::msg::PointStamped& center_point) override;

        void setParameters(rclcpp::Node* node) override;


        private:

        CoverDetectionParams params_;

        PointCloud detectHandleRegion(const PointCloud& cloud, float& detected_z);

        pcl::PointCloud<pcl::PointXYZ>::Ptr projectToXY(const PointCloud& cloud);

        bool detectHandleCenterX(const pcl::PointCloud<pcl::PointXYZ>::Ptr& xy_cloud, float& x_center, float& y_center);

        bool detectCircleCenter(const PointCloud& cloud, float& circle_x, float& circle_y);

        void mergeCenters(float handle_x, float handle_y, float circle_x, float circle_y, float& final_x, float& final_y);
    
    };
    
}

#endif